#include <iostream>
#include "SMATModel.hh"
#include "display.hh"
#include "utils.hh"
#include <string.h>

#include <pthread.h>
#include <stdlib.h>
#include <stdio.h>
#include <semaphore.h>

#include <sys/time.h>
#include <unistd.h>
#include <sys/select.h>
#include "smat-ransac.h"


#include "videoAVI.h"
#include "constants.h"
#include "face_detect_vj.hh"
#include "modelo_RANSAC.h"
#include "read_video.h"
#include "other_functions.h"
#include "options.h"
#include <colores.h>

#include "facedir.h"

#define SHOW_LEFT
//#define SHOW_RIGHT 
//#define USE_THREAD_RANSAC

int bucle_cvWaitKey(int * wait);


int main(int argc, char *argv[])
{
    if (!leer_opciones(argc, argv))
	return 0;

    CFacedir facedir;
    return facedir.main();
}

char Model_SMAT[2][100] = { "Model+SMAT", "Model+SMAT Der"};

int CFacedir::main(void)
{
    //entorno grafico  
    int i;
    CvRect haar_modelo = {223, 175, 341, 455};

    //srand( (unsigned)time( NULL ) );
    srand(1);  //siempre la misma secuencia aleatoria. Debug

    graficar = ini_opt("graficar");
    groundtruth = ini_opt("groundtruth");
    /***********/
    //lanza el hilo de lectura de las imágenes
    wait = ini_opt("wait");

    np_to_show_imgs=0;

    cvNamedWindow("SMAT", 1);
    cvNamedWindow(window[0], 1);
    cvNamedWindow(window[1], 1);

    filedata=0;
    lastframe=0;

    iniframe = ini_opt("iniframe");
    if (strcmp(ini_opts("data"), "") != 0)
    {
	filedata = fopen(ini_opts("data"), "r");
	if (filedata == NULL)
	    return -1;

	fscanf(filedata, "%d", &lastframe);
	iniframe = lastframe-1;
	//fseek(filedata, SEEK_SET, 0);
    }
    //lanza el hilo de lectura de las imágenes
    iniframe = start_ReadVideo_thread(&video_params, ini_opt("cam"), 
				      ini_opts("v"), ini_opts("format"), ini_opt("iniframe"));

    //búsqueda del fichero de calibración y 
    //  leer los parámetros de calibración
    char buffer[400];
    int num_video;
    FILE *fcal;
    char * index1, *index2;
    strcpy (buffer, video_params.videoI_name);
    index1 = rindex(buffer, '/');
    if (!index1)
	index1 = video_params.videoI_name;
    index1++;
    index2 = index(index1, '_');
    if (!index2)
    {
	printf("El nombre del fichero");
    }
    index2++;
    sscanf(index2, "%d", &num_video);
    sprintf(index2, "%d_1.cal", num_video);
    fcal = fopen(buffer, "r");
    while (fcal == NULL && num_video)
    {
	num_video--;
	sprintf(index2, "%d_1.cal", num_video);
	fcal = fopen(buffer, "r");
    }
    if (fcal == NULL)
    {
	printf("Error al buscar el fichero de calibración.\n");
	exit(-1);
    }
    fclose(fcal);

    printf("fichero de\ncalibración: %s\n", buffer);

    CvFileStorage* fs = cvOpenFileStorage( buffer, 0, CV_STORAGE_READ );
    calibration_paramsL.Fundamental = (CvMat *) cvReadByName(fs, NULL, "Fundamental");
    calibration_paramsL.R = (CvMat *) cvReadByName(fs, NULL, "R");
    calibration_paramsL.T = (CvMat *) cvReadByName(fs, NULL, "T");
    calibration_paramsL.KK = (CvMat *) cvReadByName(fs, NULL, "Fl");
    calibration_paramsR.KK = (CvMat *) cvReadByName(fs, NULL, "Fr");
    
    struct corners_params_t cp;
    cp.block_size = cvReadInt(cvGetFileNodeByName( fs, NULL, "block_size"), 19) ;
    cp.k_harris = cvReadReal(cvGetFileNodeByName( fs, NULL, "k_harris"), 0.2375);
    cp.min_distance = cvReadInt(cvGetFileNodeByName( fs, NULL, "min_distance"), 30);
    cp.quality_level_max=cvReadReal(cvGetFileNodeByName(fs, NULL, "quality_level_max"),0.006);
    cp.quality_level_min=cvReadReal(cvGetFileNodeByName(fs, NULL, "quality_level_min"),0.0000400);
    cvReleaseFileStorage( &fs );

    printf("\nModelo:%s.\n\n", ini_opts("m"));
    fflush(stdout);

    fout = NULL;
    save_video = ini_opt("ov");
    save_pose = ini_opt("op");
    videofile =0;
    struct timeval oldtimeleft={0,0}, oldtimeright={0,0};

    if (iniframe < 0)
    {
	printf("error en la lectura de los videos.\n");
	fflush(stdout);
	exit(-1);
    }
    if (!get_read_images(&imgs, &video_params, false))
    {
	printf("Error while reading images\n"); fflush(stdout);
	exit(-1);
    }

 
    vectL = cvCreateMat(3,1, CV_32FC1);
    vectR = cvCreateMat(3,1, CV_32FC1);
    vectRl = cvCreateMat(3,1, CV_32FC1);
    vect_media_L = cvCreateMat(3,1, CV_32FC1);
    vect_media_R = cvCreateMat(3,1, CV_32FC1);
    translation_media_L = cvCreateMat( 3, 1, CV_32FC1 );
    translation_media_R = cvCreateMat( 3, 1, CV_32FC1 );
    translationL = cvCreateMat( 3, 1, CV_32FC1 );
    translationR = cvCreateMat( 3, 1, CV_32FC1 );
    translationRl = cvCreateMat( 3, 1, CV_32FC1 );
    rotation = cvCreateMat( 3, 3, CV_32FC1 );


    if (ini_opt("r"))
    {
	strcpy(buffer, video_params.videoI_name);
	if (ini_opt("groundtruth"))
	    sprintf(buffer + strlen(buffer)-4, ".da%d", ini_opt("dat"));
	else
	    strcpy(buffer + strlen(buffer)-4, ".dat");
	
	if (0 == (npoints = ReadModelFromFile(buffer, &pointsID[0], &pointsID[1], &points3D, &angY, &chesscorners)))
	    exit(-1);
    }

    // si no se inidica el frame inicial, o este es el frame 0, 
    //hace un repaso para elegir el frame inicial
    if (1)//iniframe == 0)
    {
	do 
	{
	    for (i=0; i<npoints; i++)
	    {
		cvCircle (imgs->left, cvPoint((int)cvmGet(pointsID[0], i, 0), (int)cvmGet(pointsID[0], i, 1)), 7,  CV_RGB(0,0,0), 1, CV_AA, 0);
		cvCircle (imgs->right, cvPoint((int)cvmGet(pointsID[1], i, 0), (int)cvmGet(pointsID[1], i, 1)), 7,  CV_RGB(0,0,0), 1, CV_AA, 0);
	    }
	    cvShowImage(window[0], imgs->left);
	    cvShowImage(window[1], imgs->right);

	    if (!get_read_images(&imgs, &video_params, true))
	    {
		printf("Error while reading images\n"); fflush(stdout);
		exit(-1);
	    }
	} while (bucle_cvWaitKey(&wait)>=0);
    }

    if(save_video)
    {
	std::string fs_basename = get_basename(video_params.videoI_name);

	char file[fs_basename.size()+50];

	sprintf(file, "%s_results_FaceDir.avi", fs_basename.c_str());
	printf("save video en: %s\n", file);
	i=1;
	sprintf(buffer, "/scratch/Pedro/videos/video_%d.raw", i);
	while ((videofile = fopen(buffer, "r")))
	{
	    fclose(videofile);
	    i++;
	    sprintf(buffer, "/scratch/Pedro/videos/video_%d.raw", i);
	}
	videofile = fopen(buffer, "w");
	sprintf(buffer, "/scratch/Pedro/videos/data_%d.txt", i);
	fout = fopen (buffer, "w");
    }

    if(save_pose)
    {
	std::string fs_basename = get_basename(video_params.videoI_name);

	char file[fs_basename.size()+50];

	sprintf(file, "%s_pose_results.xml", fs_basename.c_str());
	pose_results.reset(cvOpenFileStorage(file, NULL, CV_STORAGE_WRITE), deleteFileStorage);		
    }


    CvSize sori = cvGetSize(imgs->left);
    CvSize sdest = sori; //{800, 800};

    img[0] = cvCreateImage(sdest, IPL_DEPTH_8U, 1);
    img[1] = cvCreateImage(sdest, IPL_DEPTH_8U, 1);

    cvCopy(imgs->left, img[0]);// = imgs->left;
    cvCopy(imgs->right, img[1]);// = imgs->right;

    sdest = cvSize(800, 800);
    img_smat[0] = cvCreateImage(sdest, IPL_DEPTH_8U, 1);
    img_smat[1] = cvCreateImage(sdest, IPL_DEPTH_8U, 1);

    CvRect roiori = {(sori.width-sdest.width)/2, 0, sdest.width, sori.height};
    CvRect roidest = {0,(sdest.height-sori.height)/2, sdest.width, sori.height};
    
    cvZero(img_smat[0]);
    cvZero(img_smat[1]);
    cvSetImageROI(imgs->left, roiori);
    cvSetImageROI(imgs->right, roiori);
    cvSetImageROI(img_smat[0], roidest);
    cvSetImageROI(img_smat[1], roidest);
    
    cvCopy(imgs->left, img_smat[0]);// = imgs->left;
    cvCopy(imgs->right, img_smat[1]);// = imgs->right;
    
    cvResetImageROI(imgs->left);
    cvResetImageROI(imgs->right);
    cvResetImageROI(img_smat[0]);
    cvResetImageROI(img_smat[1]);

    /*
      img[0] = imgs->left;
      img[1] = imgs->right;
    */

    InitModelo(&calibration_paramsL, &calibration_paramsR);
    cvShowImage(window[0], img_smat[0]);
    cvShowImage(window[1], img_smat[1]);

    printf("primer frame\n");
    cvWaitKey(10);

    //creación del modelo smat
    smatD= load_SMAT_Model(ini_opts("m"), npoints);
    smatI= load_SMAT_Model(ini_opts("m"), npoints);
    print_model_info(smatI);

    /***********************************************************************************************
     ** comienzo del algoritmo
     ***********************************************************/
    
    // leer el modelo de un archivo o crearlo
    if (!ini_opt("r"))
    {
	npoints = do_Estereo(img, 35,  
			     &pointsID[0], &pointsID[1], &points3D, &angY,
			     &calibration_paramsL, &calibration_paramsR, &cp,
			     haar_modelo, 
			     graficar);
    } 
    printf("Tamaño de la imagen: %d x %d\n", cvGetSize(img[0]).width, cvGetSize(img[0]).height);

    lines = cvCreateMat (npoints, ini_opt("lines"), CV_32FC1);
    epilines = cvCreateMat (npoints, 3, CV_32FC1);
    rot = cvCreateMat(3,3, CV_32FC1);
    mask_show = cvCreateMat(npoints, 1, CV_8UC1);

    int index_punto =0;
    struct mousecalldata_t paramsMouseCall[2];

    cvSet(mask_show, cvScalar(1));
    //cvSetReal1D(mask_show, 0, 0);

    //FindCornerSubPix(imgs->left, pointsID[0], chesscorners);
    //FindCornerSubPix(imgs->right, pointsID[1], chesscorners);

    GetStereoPoint(pointsID[0], pointsID[1], &calibration_paramsL, &calibration_paramsR, points3D);
    cvComputeCorrespondEpilines(pointsID[0], 1, calibration_paramsR.Fundamental, epilines);
    LineasModelo(points3D, lines);

    offsetmodel = CenterModel(points3D);    
    
    Modelo(points3D, pointsID[0], mask_show, calibration_paramsL.KK, rotation, translationL, vectL);
    
    cvmSet(translationL, 0,0,0);
    cvmSet(translationL, 1,0,0);

    for (i=0; i<2; i++)
    {
	image2[i] = cvCreateImage(cvGetSize(img[i]), IPL_DEPTH_8U, 3);
	cvConvertImage(img[i], image2[i]);

	paramsMouseCall[i].points = pointsID[i];
	paramsMouseCall[i].pointsI = pointsID[0];
	paramsMouseCall[i].pointsD = pointsID[1];
	paramsMouseCall[i].points3D = points3D;
	paramsMouseCall[i].epilines = epilines;
	paramsMouseCall[i].vect = vectL;
	paramsMouseCall[i].translation = translationL;
	paramsMouseCall[i].lines = lines;
	paramsMouseCall[i].calL = &calibration_paramsL;
	paramsMouseCall[i].calR = &calibration_paramsR;
	paramsMouseCall[i].image = img;
	paramsMouseCall[i].ID = i;
	paramsMouseCall[i].index = &index_punto;
	paramsMouseCall[i].angY = angY;
	paramsMouseCall[i].mask_show = mask_show;
	paramsMouseCall[i].offsetmodel = offsetmodel;
	paramsMouseCall[i].chesscorners = chesscorners;
	//cvResizeWindow(window[i], 600,600);

	cvSetMouseCallback(window[i] ,  on_mouse,  (void*)&paramsMouseCall[i]);    
    }
    
    DibujarStereo(image2, 2, pointsID[0], pointsID[1], chesscorners, NULL);
    for (i=0; i<2; i++)
    {
	cvShowImage(window[i], image2[i]);
	cvReleaseImage(&image2[i]);
    }
    
    cvSet(mask_show, cvScalar(1));
    MostrarModelo(&paramsMouseCall[0]);
    
    npoints = points3D->rows;

    //FindCornerSubPix(imgs->left, pointsID[0], chesscorners);
    //FindCornerSubPix(imgs->right, pointsID[1], chesscorners);

    printf("cvFindCornerSubPix\n");
    GetStereoPoint(pointsID[0], pointsID[1], &calibration_paramsL, &calibration_paramsR, points3D);
    LineasModelo(points3D, lines);

    //centra el modelo sobre el punto 0 para un correcto uso de cvPOSIT / cvProjectPoints2
    offsetmodel = CenterModel(points3D);    
    
    /*
      pointsID[0] = paramsMouseCall[0]->pointsI;
      pointsID[1] = p->pointsD;
      points3D = p->points3D;
      epilines = p->epilines;
      lines = p->lines;
      angY = p->angY;
      mask_show = p->mask_show;
    */
    cvDestroyWindow(window[1]);

    //mostrar modelos iniciales sobre las imágenes
    if (1)
    {
	cvSet(mask_show, cvScalar(1));
	cvSetReal1D(mask_show, 0, 0);
	Modelo(points3D, pointsID[0], chesscorners, calibration_paramsL.KK, rotation, translationL, vectL);

	offsettrans = cvCloneMat(translationL);
	offsettrans2 = cvCloneMat(translationL);
	x2 = &offsettrans->data.fl[0];
	y2 = &offsettrans->data.fl[1];
	z2 = &offsettrans->data.fl[2];
	printf ("offsettrans: frame %d -> (%f %f %f)\n", lastframe, *x2, *y2, *z2);

	cvNamedWindow("Modelo3D Der", 0);
	cvResizeWindow("Modelo3D Der", 600,600);
	cvNamedWindow("Modelo3D", 0);
	cvResizeWindow("Modelo3D", 600,600);

	IplImage *clonekI = cvCreateImage(cvGetSize(img[0]), IPL_DEPTH_8U, 3);
	IplImage *clonekD = cvCreateImage(cvGetSize(img[1]), IPL_DEPTH_8U, 3);
	cvSet(mask_show, cvScalar(1));

	cvConvertImage(img[0], clonekI);
	DibujarModelo3D(clonekI, points3D,  &calibration_paramsL, translationL, vectL,  lines, mask_show);
	cvShowImage("Modelo3D", clonekI);
	// obtenemos vectR y translationR en coordenadas de la cámara derecha
	ChangeView(vectL, translationL, &calibration_paramsR, vectR, translationR, rot);
	//Modelo(points3D, pointsID[1], NULL, calibration_paramsR.KK, rotation, translationR, vectR);
	cvConvertImage(img[1], clonekD);
	DibujarModelo3D(clonekD, points3D,  &calibration_paramsR, translationR, vectR,  lines, mask_show);
	cvShowImage("Modelo3D Der", clonekD);

	// obtenemos vectR y translationR en coordenadas de la cámara derecha
	// 	ChangeView(vectR, translationR, &calibration_paramsL, vectL, translationL, rot);
	// 	cvConvertImage(img[0], clonekI);
	// 	DibujarModelo3D(clonekI, points3D,  &calibration_paramsL, translationL, vectL,  lines, mask_show);
	// 	cvShowImage("Modelo3D", clonekI);

	cvConvertImage(img[0], clonekI);
	DibujarModelo(clonekI, pointsID[0], &calibration_paramsL, translationL, vectL,  lines, mask_show);
	cvShowImage(Model_SMAT[0], clonekI);
	cvWaitKey(0);

	cvDestroyWindow("Modelo3D");
	cvDestroyWindow("Modelo3D Der");

	cvReleaseImage(&clonekI);
	cvReleaseImage(&clonekD);
    }
    

    
#ifdef SHOW_LEFT
    cvNamedWindow(Model_SMAT[0], 1);
    cvResizeWindow(Model_SMAT[0], 600,600);
#endif
#ifdef SHOW_RIGHT
    cvNamedWindow(Model_SMAT[1], 1);
    cvResizeWindow(Model_SMAT[1], 600,600);
#endif

    
    Modelo(points3D, pointsID[0], chesscorners, calibration_paramsL.KK, rotation, translationL, vectL);

    // pasamos de vectI a vectR, a coordenadas de la cámara derecha
    ChangeView(vectL, translationL, &calibration_paramsR, vectR, translationR, rot);
    cvCopy (translationL, translation_media_L);
    cvCopy (vectL, vect_media_L);

    // desplazamos los puntos del modelo para que el SMAT use el punto en el centro, no en la esquina.
    desplazar_parche = cvScalar( 15 + roiori.x , 15 - roidest.y, 0, 0);
    cvReshape(pointsID[0], &pointsID2[0], 2, 0);
    cvReshape(pointsID[1], &pointsID2[1], 2, 0);
    cvSubS(&pointsID2[0], desplazar_parche, &pointsID2[0]);
    cvSubS(&pointsID2[1], desplazar_parche, &pointsID2[1]);

    boost::shared_ptr<CvMat> mp (cvCreateMat(2, pointsID[0]->rows, pointsID[0]->type), deleteCvMat);
    smat_pts.push_back(mp);
    mp.reset(cvCreateMat(2, pointsID[0]->rows, pointsID[0]->type), deleteCvMat);
    smat_pts.push_back(mp);
    
    // REdesplazamos los puntos del modelo para que el SMAT use el punto en el centro, no en la esquina.
    cvAddS(&pointsID2[0], desplazar_parche, &pointsID2[0]);
    cvAddS(&pointsID2[1], desplazar_parche, &pointsID2[1]);


    //    CvMat *smat_mask = cvCreateMat(smatI->elements.size(), 1, CV_8U);
    //     cvSet(mask, cvScalar(1));
    //     cvSetReal1D(mask, 3, 0);
    
    maskL = cvCreateMat(npoints, 1, CV_8UC1);	    	    
    maskR = cvCreateMat(npoints, 1, CV_8UC1);	    	    
    maskoutI = cvCreateMat(npoints, 1, CV_8UC1);	    	    
    maskoutD = cvCreateMat(npoints, 1, CV_8UC1);
    maskoutI2 = cvCreateMat(npoints, 2, CV_8UC1);
    maskoutD2 = cvCreateMat(npoints, 2, CV_8UC1);

    points2D_reprojectI =  cvCreateMat(npoints, 2, CV_32FC1); 
    points2D_reprojectD =  cvCreateMat(npoints, 2, CV_32FC1); 


    maskoutL = cvCreateMat(npoints, 1, CV_8UC1);	    	    
    maskoutL2 = cvCreateMat(npoints, 2, CV_8UC1);
    masktemp = cvCreateMat(npoints, 1, CV_8UC1);	

    frame = 0;
    quit = false;

    sem_init(&sem1, 0, 0);
    sem_init(&sem2, 0, 0);

    params_left.smat_pts = &smat_pts.at(0);
    params_left.smat = &smatI;
    params_left.src = &srcI;
    params_left.mask_show = maskL;
    params_left.maskout = maskoutI;
    params_left.pointsID = pointsID[0];
    params_left.inliers =0;
    params_left.points3D = points3D;
    params_left.cal = &calibration_paramsL;
    params_left.translation = translationL;
    params_left.vect = vectL;
    params_left.vect_media = vect_media_L;
    params_left.quit = 0;
    params_left.desplazar_parche = desplazar_parche;
    params_left.angY = angY;
    params_left.sem1 = &sem1;
    params_left.sem2 = &sem2;
    params_left.n = 0;
    params_left.img = img[0];
    params_left.img_smat = img_smat[0];
    params_left.chesscorners = chesscorners;

    params_right.img = img[1];
    params_right.img_smat = img_smat[1];
    params_right.n = 1;
    params_right.smat_pts = &smat_pts.at(1);
    params_right.smat = &smatD;
    params_right.src = &srcD;
    params_right.mask_show = maskR;
    params_right.maskout = maskoutD;
    params_right.pointsID = pointsID[1];
    params_right.inliers =0;
    params_right.points3D = points3D;
    params_right.cal = &calibration_paramsR;
    params_right.translation = translationR;
    params_right.vect = vectR;
    params_right.vect_media = vect_media_R;
    params_right.quit = 0;
    params_right.desplazar_parche = desplazar_parche;
    params_right.angY = angY;
    params_right.sem1 = &sem1;
    params_right.sem2 = &sem2;
    params_right.chesscorners = chesscorners;

    //*************************************************************

    key =0;
    wait = ini_opt("wait");
    cvDestroyWindow("Camara Izq");
    offsetvect = 0;
    //vector inicial
    if (strlen(ini_opts("inivect")))
    {
	offsetvect = cvCloneMat(vectL);
	float ai, bi, gi;
	sscanf(ini_opts("inivect"), "%f,%f,%f", &ai, &bi, &gi);
	cvmSet(offsetvect, 0,0, ai/180*CV_PI);
	cvmSet(offsetvect, 1,0, bi/180*CV_PI);
	cvmSet(offsetvect, 2,0, gi/180*CV_PI);
    }

    //inicializaci󮠤el buffer de SHOW_IMAGE_BUFFER_SIZE imagenes para poder ir hacia atras
    np_to_show_imgs = 0;
    for (i=0; i<SHOW_IMAGE_BUFFER_SIZE; i++)
    {
	buffer_show_imgs[i] = cvCreateImage(cvGetSize(params_left.img), IPL_DEPTH_8U, 3);
	buffer_display_smat[i] = cvCreateImage(DISPLAY_SIZE, IPL_DEPTH_32F, 3);
    }

    CvMat * pointsID_old[2];
    pointsID_old[0] = cvCloneMat(pointsID[0]);
    pointsID_old[1] = cvCloneMat(pointsID[1]);

    modificado=0; 
    modificando=0;
    incremento=0.5/180.0*CV_PI;
    incrementopos = 2;
    //float basura;
    vect_2 = cvCloneMat(vectL);

    a = &vect_media_L->data.fl[0];
    b = &vect_media_L->data.fl[1];
    c = &vect_media_L->data.fl[2];

    x = &translation_media_L->data.fl[0];
    y = &translation_media_L->data.fl[1];
    z = &translation_media_L->data.fl[2];

    a2 = &vect_2->data.fl[0];
    b2 = &vect_2->data.fl[1];
    c2 = &vect_2->data.fl[2];

    //cvZero(offsettrans);
    oldtrans = cvCloneMat(translation_media_L);
    oldvect = cvCloneMat(vect_media_L);

    GetNotHiden(vectL, angY, maskL);
    GetNotHiden(vectR, angY, maskR);

    while(!quit)
    {
	if (!get_read_images(&imgs, &video_params, true))
	    break;
	
	if (imgs->hleft->nframe == 312)
	    printf("frame 312\n");

	frame2 = imgs->hleft->nframe;
	cvCopy(imgs->left, img[0]);// = imgs->left;
	cvCopy(imgs->right, img[1]);// = imgs->right;

	cvCopy (pointsID[0], pointsID_old[0]);
	cvCopy (pointsID[1], pointsID_old[1]);
	
	cvZero(img_smat[0]);
	cvZero(img_smat[1]);
	cvSetImageROI(imgs->left, roiori);
	cvSetImageROI(imgs->right, roiori);
	cvSetImageROI(img_smat[0], roidest);
	cvSetImageROI(img_smat[1], roidest);
        
	cvCopy(imgs->left, img_smat[0]);// = imgs->left;
	cvCopy(imgs->right, img_smat[1]);// = imgs->right;

	cvResetImageROI(imgs->left);
	cvResetImageROI(imgs->right);
	cvResetImageROI(img_smat[0]);
	cvResetImageROI(img_smat[1]);
	
	oldtimeright = imgs->hright->filltime;
	oldtimeleft = imgs->hleft->filltime;

	cvCopy(translation_media_L, oldtrans);
	cvCopy(vect_media_L, oldvect);

	np_to_show_imgs++;
	np_to_show_imgs %= SHOW_IMAGE_BUFFER_SIZE;
	clonek = buffer_show_imgs[np_to_show_imgs];
	display = buffer_display_smat[np_to_show_imgs];

	if (!frame)
	{
	    facedir_control();
	}

	if (1)//!frame)
	{
	    SMAT_RANSAC(&params_left);
	    SMAT_RANSAC(&params_right);
	}
	inliersI = params_left.inliers;
	inliersD = params_right.inliers;

	display_SMATModel(display, srcI.get(), smatI);
	    
	facedir();

	facedir_control();

	frame ++;
    }

    //libera los buffers que se crearon para poder revobinar
    for (i=0; i<SHOW_IMAGE_BUFFER_SIZE; i++)
    {
	cvReleaseImage(&buffer_show_imgs[i]);
	cvReleaseImage(&buffer_display_smat[i]);
    }
    if (fout)
	fclose(fout);

    if (videofile)
	fclose(videofile);
    return 0;
}

struct option2 mainopts2[]= {
    {{"v", 1, NULL, OPT_VAL_IS_CHAR}, 0, "", 
     "Video to process"},
    {{"m", 1, NULL, OPT_VAL_IS_CHAR}, 0, "../data/model_test_31.xml",
     "The file containing the SMAT definition."},
    {{"format", 0, NULL, OPT_VAL_IS_CHAR}, 0, "", 
     "format = [raw | ra2 | avi]: Tells the format of the video"},
    {{"op", 0, NULL, true}, false, "", 
     "Saves the pose values after each frame. The file name will be <FILE>_pose_results.xml"},
    {{"ov", 0, NULL, true}, false, "", 
     "Saves a video of the results. They will be stored in video_results_SMAT.avi"
     " and video_results_FaceDir.avi"}, 
    {{"lines", 1, NULL, OPT_VAL_IS_INT}, 4, "", 
     "tIndicates de number of lines to draw which connects from each node"},
    {{"r", 0, NULL, true}, false, "", 
     "Read Model: Does not autocreate the Model. Reads if from a file"},
    {{"wait", 1, NULL, OPT_VAL_IS_INT}, 0, "", 
     "tIndicates de parameter for the cvWaitKey(n)"},
    {{"cam", 0, NULL, true}, false, "", 
     "Capture de video from the cameras"},

    {{"groundtruth", 0, NULL, true}, false, "", 
     "procesado del groundtruth"},
    {{"dat", 1, NULL, OPT_VAL_IS_INT}, 2, "", 
     "indica el numero N del archivo .daN a leer como datos del modelo"},
    {{"data", 1, NULL, OPT_VAL_IS_CHAR}, 0, "", 
     "..."},
    {{"graficar", 0, NULL, true}, false, "", 
     "Muestra le proceso de matching 3D"},

    {{"options", 0, NULL, true}, false, "", 
     "Imprime las opciones de configuracion del programa"},
    {{"h", 0, NULL, true}, false, "", 
     ""},
    {{"inivect", 1, NULL, OPT_VAL_IS_CHAR}, 0, "",
     "Vector inicial de direcicón"},

    {{"iniframe", 1, NULL, OPT_VAL_IS_INT}, -1, "",
     "inidica el frame de inicio"},
    {{"h", 0, NULL, true}, false, "", 
     "Muestra las opciones que se pueden indicar por línea de comandos"},
    {{"help", 0, NULL, true}, false, "", 
     "Muestra esta ventana de ayuda"},
    {{NULL, 0, NULL, 0}, 0, "", 
     ""}
};
